#include <Arduino.h>

// 1/20 秒，50Hz 的频率，20ms 的周期
constexpr int freq = 50;
// 通道
constexpr int yaw_channel = 0;
constexpr int pitch_channel = 1;
// 分辨率设置为 12，用 4096 的数值来映射角度
constexpr int resolution =  12;
// 定义舵机 PWM 控制引脚。 
constexpr int yaw_servo = 13;
constexpr int pitch_servo = 12;

// yaw 93 中心 最左角143 最右角43
// pitch 25 中心 最仰角0 最俯角50

//定义函数用于输出 PWM 的占空比
int degreetoPWM(int degree)
{
    //20ms 周期内，高电平持续时长 0.5-2.5 ms，对应 0-180 度舵机角度
    float min_width = 0.5 / 20 * pow(2, resolution);
    float max_width = 2.5 / 20 * pow(2, resolution);
    if (degree < 0)
      degree = 0;
    if (degree > 180)
      degree = 180;
    return (int)(((max_width - min_width) / 180) * degree + min_width); 
}

void setup()
{
    Serial.begin(9600);
    // yaw servo
    ledcSetup(yaw_channel, freq, resolution);
    ledcAttachPin(yaw_servo, yaw_channel);
    // pitch servo
    ledcSetup(pitch_channel, freq, resolution);
    ledcAttachPin(pitch_servo, pitch_channel);
}

void loop()
{
    // test
    static int degree = 90;

    if (Serial.available())
    {
        char buffer[8] = { 0 };
        Serial.print(Serial.readBytes(buffer, 3));
        degree = (buffer[0] - '0') * 100 + (buffer[1] - '0') * 10 + buffer[2] - '0';
        Serial.print(' ');
        Serial.println(degree);
    }

    ledcWrite(pitch_channel, degreetoPWM(degree));
    ledcWrite(yaw_channel, degreetoPWM(degree));
}